/*
    COMP9517 Computer Vision, Semester 2, 2011 - University of New South Wales
   ============================================================================
   |   Group .  | Christian Mostegel & Dong Back Kim                          |
   ============================================================================
 */

#include "ImageAnalyzer.h"
#include "KDTree.h"

#include <iostream>
#include <ctime>
#include <sys/time.h>


#define SURF 0
#define SIFT 1
//#

using namespace cv;
using namespace std;

struct match_sort
{
    bool operator() (const cv::DMatch & a, const cv::DMatch & b)
    {
        if ( a.trainIdx < b.trainIdx )
        {
            return true;
        }
        else
        {
            if ( a.trainIdx == b.trainIdx )
            {
                if ( a.distance < b.distance )
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else
            {
                return false;
            }
        }
    }
};

ImageAnalyzer::ImageAnalyzer() {
    int mode = SIFT;
    if(mode == SURF)
    {
        fd_ = FeatureDetector::create("SURF");
        desc_extractor_ = new cv::SurfDescriptorExtractor();
    }
    else if(mode == SIFT)
    {
        fd_ = FeatureDetector::create("SIFT");
        desc_extractor_ = new cv::SiftDescriptorExtractor();
    }
    max_distance_ = __FLT_MAX__;//0.2;
    total_detection_time_ = 0;
    total_desc_time_ = 0;
    total_num_features_ = 0;
    total_match_time_ = 0;

}

ImageAnalyzer::~ImageAnalyzer() {
    //fd_.delete_obj();
}


void ImageAnalyzer::detectKeyPoints( const Mat& image, vector<KeyPoint>& keypoints )
{
    timespec start,end;
    QTime startTime, endTime;

    startTime = QTime::currentTime();

    clock_gettime(CLOCK_REALTIME, &start);

    fd_->detect(image,keypoints);

    clock_gettime(CLOCK_REALTIME, &end);

    //cout << "start =" << start.tv_nsec << endl;
    //cout << "end   =" << end.tv_nsec << endl;
   // cout << "dectection time =" << (end.tv_nsec - start.tv_nsec) << endl;

    endTime = QTime::currentTime();

    //cout << "Started  at = " << startTime.msecsTo(startTime) << endl;
    //cout << "Finished at = " << startTime.msecsTo(endTime) << endl;
    cout << "Elapsed     = " << startTime.msecsTo(endTime) << " ms"<< endl;

    total_detection_time_ += (end.tv_nsec-start.tv_nsec);

  //  cout << "total_detection_time =" << total_detection_time_ << endl;
    total_num_features_ += keypoints.size();
}

void ImageAnalyzer::computeDescriptors( const Mat& image, vector<KeyPoint>& keypoints, Mat& descriptors )
{
    timespec start,end;
    clock_gettime(CLOCK_REALTIME, &start);
    desc_extractor_->compute(image,keypoints,descriptors);

    clock_gettime(CLOCK_REALTIME, &end);
    total_desc_time_ += (end.tv_nsec-start.tv_nsec);
}


void  ImageAnalyzer::match(const AnalyzedImage & img1, const AnalyzedImage & img2, std::vector<cv::DMatch>& matches)
{

    timespec start,end;
    clock_gettime(CLOCK_REALTIME, &start);
    crossCheckMatch(img1.desc,img2.desc,matches,max_distance_);
    /* for(int i = 0 ; i< matches.size();i++)
    {
        cout << "dist: " << matches[i].distance << endl;
    }*/
    //cout << " Image 1 descriptors = " << *(img1.desc.size.p) << endl;
    //cout << " Image 2 descriptors = " << *(img2.desc.size.p) << endl;

    //kdTreeMatch(img1,img2,matches,max_distance_);

    clock_gettime(CLOCK_REALTIME, &end);
    total_match_time_ += (end.tv_nsec-start.tv_nsec);
}

void ImageAnalyzer::kdTreeMatch( const AnalyzedImage & img1, const AnalyzedImage & img2, std::vector<cv::DMatch>& matches, float maxDistance)
{

    img1.kd_tree->match(img1.desc, img2.desc, matches, maxDistance);

    /*cout << "Matches found = " << common_matches.size() << endl;

    match_sort sorter;

    std::sort(common_matches.begin(), common_matches.end(), sorter);

    for(int i=0; i < common_matches.size(); i++)
    {
        cout << "[" << i << "] src = " << common_matches[i].trainIdx
             << " dst = " << common_matches[i].queryIdx
             << " distance = " << common_matches[i].distance << endl;
    }

    cout << "DONE" << endl;*/
}

void ImageAnalyzer::crossCheckMatch( const cv::Mat& desc1, const cv::Mat& desc2,
                                     std::vector<cv::DMatch>& common_matches, float maxDistance)
{
    vector<DMatch> single_matches;
    vector<DMatch> single_reverse_matches;
    vector< vector<DMatch> > matches;
    vector< vector<DMatch> > reverseMatches;

    desc_matcher_.radiusMatch(desc1, desc2, matches, maxDistance);
    desc_matcher_.radiusMatch(desc2, desc1, reverseMatches, maxDistance);

    for(size_t k = 0; k < matches.size(); k++)
    {
        vector<DMatch> point_matches = matches.at(k);

        if(point_matches.size() > 0)
        {
            single_matches.push_back(point_matches.at(0));
        }
    }

    for(size_t k = 0; k < reverseMatches.size(); k++)
    {
        vector<DMatch> point_matches = reverseMatches.at(k);

        if(point_matches.size() > 0)
        {
            single_reverse_matches.push_back(point_matches.at(0));
        }
    }

    for(size_t k=0; k < single_matches.size(); k++)
    {
        for(size_t m=0; m < single_reverse_matches.size(); m++)
        {
            if ( single_matches[k].queryIdx == single_reverse_matches[m].trainIdx
                 && single_matches[k].trainIdx == single_reverse_matches[m].queryIdx)
            {
                common_matches.push_back(single_matches[k]);
            }
        }
    }

}

void ImageAnalyzer::getSingleMatches(vector<vector<DMatch> > & matches1to2 ,vector<DMatch> & single_matches1to2)
{
    for(int i = 0; i < (int)matches1to2.size(); i++)
    {
        vector<DMatch> point_matches = matches1to2.at(i);
        if(point_matches.size() > 0)
        {
            single_matches1to2.push_back(point_matches.at(0));
        }
    }

}

void ImageAnalyzer::getSingleReversedMatches(vector<vector<DMatch> > & matches2to1 ,vector<DMatch> & single_matches1to2)
{
    for(int i = 0; i < (int)matches2to1.size(); i++)
    {
        vector<DMatch> point_matches = matches2to1.at(i);
        if(point_matches.size() > 0)
        {
            DMatch match2to1 = point_matches.at(0);
            DMatch match1to2 = match2to1;
            match1to2.queryIdx = match2to1.trainIdx;
            match1to2.trainIdx =  match2to1.queryIdx;
            single_matches1to2.push_back(match1to2);
        }
    }

}
